﻿using System;
using System.Collections.Generic;
using System.Text;
using PotentialFieldDriver.robotrace.environment.interfaces;
using PotentialFieldDriver.robotrace.utils.pitch;
using Geometry;
using PotentialFieldDriver.robotrace.utils.geometry;
using System.Drawing;

namespace PotentialFieldDriver.robotrace.utils.fields
{
    class PotentialField
    {
        public const double DEFAULT_WALL_POTENTIAL = 100.0;
        public const int DEFAULT_WALL_POTENTIAL_INFLUENCE_RADIX = 5;
        private double[][] field;
        private double wallPotential;
        private int wallPotentialInfluenceRadix = DEFAULT_WALL_POTENTIAL_INFLUENCE_RADIX;
        private Bitmap bitmap;
        public Bitmap PotentialBitmap
        {
            get { return bitmap; }
        }

        public PotentialField(PitchInterface pitch, Point2D destination, int wallInfluenceRadix = DEFAULT_WALL_POTENTIAL_INFLUENCE_RADIX, double wallPotential = DEFAULT_WALL_POTENTIAL)
        {
            Cell[][] obstaclesBitmap = pitch.getObstaclesBitmap();
            this.wallPotential = wallPotential;
            this.wallPotentialInfluenceRadix = wallInfluenceRadix;
            initializePotentialField(wallPotential, obstaclesBitmap);
            int robotX = pitch.mapRobotXPositionToCellColumnNo(destination.x);
            int robotY = pitch.mapRobotYPositionToCellRowNo(destination.y);
            computePotential(obstaclesBitmap, robotX, robotY);
            prepareBitmap(robotX, robotY, obstaclesBitmap.Length, obstaclesBitmap[0].Length, obstaclesBitmap);
        }

        public double getValue(int column, int row)
        {
            if (column > field.Length || column < 0 || row > field[column].Length || row < 0)
            {
                return wallPotential;
            }
            return field[column][row];
        }

        private void prepareBitmap(int robotX, int robotY, int width, int height, Cell[][] obstaclesBitmap)
        {
            Bitmap potentialFieldBitmap = new Bitmap(width, height);
            for (int i = 0; i < width; i++)
            {
                for (int j = 0; j < height; j++)
                {
                    int red = Convert.ToInt32(Math.Max(255 * (field[i][j] / wallPotential), 0));
                    int blue = 255 - red;//Convert.ToInt32(255 * (0.5 - Math.Abs(field[i][j] / wallPotential - 0.5)) * 2);
                    int green = 0;// Convert.ToInt32(Math.Max(255 * GeometryUtils.sqr(1.0 - field[i][j] / wallPotential), 0));
                    if (obstaclesBitmap[i][j].Reachable)
                    {
                        potentialFieldBitmap.SetPixel(i, j, Color.FromArgb(red, green, blue));
                    }
                    else
                    {
                        potentialFieldBitmap.SetPixel(i, j, Color.Goldenrod);
                    }
                }
            }
            potentialFieldBitmap.SetPixel(robotX, robotY, Color.FromArgb(0, 0, 255));
            this.bitmap = potentialFieldBitmap;
        }

        private void computePotential(Cell[][] obstaclesBitmap, int destinationColumnIdx, int destinationRowIdx)
        {
            double maxDistance = Math.Max(obstaclesBitmap.Length, obstaclesBitmap[0].Length);//Math.Sqrt(GeometryUtils.sqr(obstaclesBitmap.Length) + GeometryUtils.sqr(obstaclesBitmap[0].Length));
            for (int columnIdx = 0; columnIdx < field.Length; columnIdx++)
            {
                for (int rowIdx = 0; rowIdx < field[columnIdx].Length; rowIdx++)
                {
                    if (obstaclesBitmap[columnIdx][rowIdx].isObstacle())
                    {
                        field[columnIdx][rowIdx] = wallPotential;
                    }
                    else
                    {
                        double distance = Math.Sqrt(GeometryUtils.sqr((double)(columnIdx - destinationColumnIdx)) + GeometryUtils.sqr((double)(rowIdx - destinationRowIdx)));
                        field[columnIdx][rowIdx] -= wallPotential * GeometryUtils.sqr(GeometryUtils.sqr(1 - (distance / maxDistance)));
                        for (int wallInfluenceColumnIdx = -wallPotentialInfluenceRadix; wallInfluenceColumnIdx < wallPotentialInfluenceRadix + 1; wallInfluenceColumnIdx++)
                        {
                            if (wallInfluenceColumnIdx + columnIdx < 0 || wallInfluenceColumnIdx + columnIdx >= obstaclesBitmap.Length)
                            {
                                continue;
                            }
                            for (int wallInfluenceRowIdx = - wallPotentialInfluenceRadix; wallInfluenceRowIdx < wallPotentialInfluenceRadix+1; wallInfluenceRowIdx++)
                            {
                                if (wallInfluenceRowIdx + rowIdx < 0 || wallInfluenceRowIdx + rowIdx >= obstaclesBitmap[wallInfluenceColumnIdx + columnIdx].Length)
                                {
                                    continue;
                                }
                                if (obstaclesBitmap[wallInfluenceColumnIdx + columnIdx][wallInfluenceRowIdx + rowIdx].isObstacle())
                                {
                                    double wallDistance = Math.Sqrt(GeometryUtils.sqr((double)(wallInfluenceColumnIdx)) + GeometryUtils.sqr((double)(wallInfluenceRowIdx)));
                                    double tmp = wallDistance / (Math.Sqrt(2) * (double)wallPotentialInfluenceRadix);
                                    field[columnIdx][rowIdx] += wallPotential * (1.0 - GeometryUtils.sqr(GeometryUtils.sqr(tmp))) / 100.0;
                                }
                            }
                        }
                    }
                    field[columnIdx][rowIdx] = Math.Max(Math.Min(field[columnIdx][rowIdx], wallPotential), 0);
                }

            }
        }

        private void initializePotentialField(double wallPotential, Cell[][] obstaclesBitmap)
        {
            this.field = new double[obstaclesBitmap.Length][];
            for (int i = 0; i < obstaclesBitmap.Length; i++)
            {
                field[i] = new double[obstaclesBitmap[i].Length];
                for (int j = 0; j < field[i].Length; j++)
                {
                    field[i][j] = wallPotential;
                }
            }
        }
    }
}
